#include "precompiled.h"

#include "Config.h"
#include "Util.h"
#include "Environment.h"
#include "Robot.h"
#include "Robomag.h"

using namespace boost::numeric::ublas;

Robomag::Robomag() :
	MaxSpeed (500),
	MaxWheelAngle (0.34906585),
	Width (15.0),
	Length (25.0),
	CenterOfMass (12.0),
	Kp (10),
	Ki (0),
	Kd (0),
	mHasReachedGoal(false),
	mCurGoal(0)
{
	for(int i = 0; i < NUM_GOALS; i++) {
		goals[i].resize(2);
	}

	goals[0][0] = 50;
	goals[0][1] = 50;
	
	goals[1][0] = 900;
	goals[1][1] = 900;
	
	goals[2][0] = 1400;
	goals[2][1] = 50;
	
	goals[3][0] = 50;
	goals[3][1] = 900;
}

Robomag::~Robomag() {
	
}

// sets the mSpeed by clamping within range
void Robomag::SetSpeed(double value) {
	// TODO: introduce time delay
	mSpeed = Util::clamp<double>(value, 0, 1.0);
}

// sets the wheel angle by claming within max range
void Robomag::SetWheelAngle(double value) {
	mDirection = Util::clamp(value, -1.0, 1.0);
}

void Robomag::Start() {
	// the robot is now allow to ask the environment questions,
	// like the magnetic field strength at certain points

	SetSpeed(1.0);		// set full speed ahead
	SetWheelAngle(0);
	mPreviousError = 0;
	mIntegral = 0;
	mDirection = 0;

}

//Every step, we are allowed to update our speed and direction, that's it.
void Robomag::Step(double t, double dt) {
	vec curPos = mEnv->GetCarPos();
	double curDir = mEnv->GetCarDirection();
	
	if(mHasReachedGoal) {
		SetSpeed(0);
		SetWheelAngle(0);
		return;
	}

	vec curGoal = goals[mCurGoal];

	// have we reached our goal?
	if(norm_2(curGoal - curPos) < 10) {
		mCurGoal++;
		if(mCurGoal == NUM_GOALS) {
			mHasReachedGoal = true;
		}
	}

	// determine delta theta
	vec v1(2);
	v1[0] = cos(curDir);
	v1[1] = sin(curDir);

	vec v2(2);
	v2 = curGoal - curPos;

	// angle of 2 relative to 1= atan2(v2.y,v2.x) - atan2(v1.y,v1.x)
	double dTheta = atan2(v2[1], v2[0]) - atan2(v1[1], v1[0]);
	mError = dTheta;

	// two modes: linear and nonlinear
	// want to determine

	double error = mError;
	mIntegral += (error * dt);
	double derivative = (error - mPreviousError) / dt;
	mOutput = (Kp * error) + (Ki * mIntegral) + (Kd * derivative);
	mPreviousError = error;

	SetWheelAngle(mOutput);

}

void Robomag::Save(SimResult::Data &data) {
	
}
